/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: CoordinateDriveData.cpp,v 1.16 2009-09-25 09:11:24 morsko Exp $
 *
 * \file
 * \brief Implementation for CoordinateDrive data encoder/decoder
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include "CoordinateDriveData.hpp"
#include "binbag.h"
#include <stdio.h>
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
using namespace MaCI::CoordinateDrive;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CCoordinateDriveData::CCoordinateDriveData()
  : iValid(false),
    iTimestampPtr(NULL),
    iFirstCoordinatePtr(NULL),
    iTargetLimitDistancePtr(NULL),
    iDriveModePtr(NULL),
    iCommandPtr(NULL),
    iCoordinateHeaderPtr(NULL),
    iDriveModeHeaderPtr(NULL),
    iSpeedPtr(NULL),
    iCoordinateElementCount(0),
    iHeaderCoordinateElementCount(0),
    iDriveModeElementCount(0),
    iHeaderDriveModeElementCount(0){}
//*****************************************************************************

CCoordinateDriveData::~CCoordinateDriveData()
{
}
//*****************************************************************************

CCoordinateDriveData::CCoordinateDriveData(const CCoordinateDriveData &aCoordinateDriveData)
  : iValid(false),
    iTimestampPtr(NULL),
    iFirstCoordinatePtr(NULL),
    iTargetLimitDistancePtr(NULL),
    iDriveModePtr(NULL),
    iCommandPtr(NULL),
    iCoordinateHeaderPtr(NULL),
    iDriveModeHeaderPtr(NULL),
    iSpeedPtr(NULL),
    iCoordinateElementCount(0),
    iHeaderCoordinateElementCount(0),
    iDriveModeElementCount(0),
    iHeaderDriveModeElementCount(0)
{

  *this = aCoordinateDriveData;
}
//*****************************************************************************

CCoordinateDriveData &CCoordinateDriveData::operator=(const CCoordinateDriveData &aCoordinateDriveData)
{
  
  // Reset
  Reset();
  // Create an internal BinBag instance
  if (CreateInternalBinBag(aCoordinateDriveData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aCoordinateDriveData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"WARNING: Failed to Decode CoordinateDriveData even though assignment operator used?");
  }

  return *this; 

}
//*****************************************************************************
bool CCoordinateDriveData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess
  if (aBinBag != NULL)  
    iBinBagContainer = aBinBag;
  
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeTimestamp:
	if (iTimestampPtr == NULL) {
	  iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);
	} else result = false;
	break;
      case KTypeCoordinate2D:
	if(iFirstCoordinatePtr ==NULL && iCoordinateHeaderPtr != NULL&&
	   previous_type == KTypeCoordinateHeader){
	  iFirstCoordinatePtr = reinterpret_cast<const TCoordinate2D *>(b->data);
	  iCoordinateElementCount++;
	}else if(iFirstCoordinatePtr == NULL){
	  iFirstCoordinatePtr = reinterpret_cast<const TCoordinate2D *>(b->data);
	  iCoordinateElementCount++;
	}else if(iFirstCoordinatePtr != NULL &&previous_type == KTypeCoordinate2D){
	  iCoordinateElementCount++;
	}else{

	  result = false;
	}
	break;
      case KTypeTargetLimitDistance:
	if(iTargetLimitDistancePtr == NULL){
	  iTargetLimitDistancePtr = reinterpret_cast<const TTargetLimitDistance *>(b->data);
	}else{

	  result = false;
	}
	break;

      case KTypeDriveMode:
	if(iDriveModePtr == NULL && iDriveModeHeaderPtr != NULL &&
	   previous_type == KTypeDriveModeHeader){
	  iDriveModePtr = reinterpret_cast<const TDriveMode *>(b->data);
	  iDriveModeElementCount++;
	}else if(iDriveModePtr == NULL){
	  iDriveModePtr = reinterpret_cast<const TDriveMode *>(b->data);
	  iDriveModeElementCount++;	
	}else if(iDriveModePtr != NULL && previous_type == KTypeDriveMode){
	  iDriveModeElementCount++;
	}else{

	  result = false;
	}

	break;

      case KTypeCommand:
        if (iCommandPtr == NULL) {
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data);
        } else {

	  result = false;
	}
        break;

      case KTypeCoordinateHeader:
        if (iCoordinateHeaderPtr == NULL) {
          iCoordinateHeaderPtr = reinterpret_cast<const TCoordinateHeader *>(b->data);
          iHeaderCoordinateElementCount = iCoordinateHeaderPtr->coordinateCount;
        } else{

	  result = false;
	}
        break;

      case KTypeDriveModeHeader:
        if (iDriveModeHeaderPtr == NULL) {
          iDriveModeHeaderPtr = reinterpret_cast<const TDriveModeHeader *>(b->data);
          iHeaderDriveModeElementCount = iDriveModeHeaderPtr->driveModeCount;
        } else{

	  result = false;
	}
        break;

      case KTypeSpeed:
        if(iSpeedPtr == NULL){
          iSpeedPtr = reinterpret_cast<const TSpeed *>(b->data);
        }else{
          result = false;
        }
        break;
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else result = false;
  
  // Store the result
  SetReadOnlyContainer( (iValid = result) );
  
  return result;
  
}
//*****************************************************************************

void CCoordinateDriveData::Reset()
{
  CMaCIData::Reset();

  iValid = false;
  iTimestampPtr = NULL;
  iFirstCoordinatePtr = NULL;
  iTargetLimitDistancePtr = NULL;
  iDriveModePtr = NULL;
  iCommandPtr = NULL;
  iCoordinateHeaderPtr = NULL;
  iDriveModeHeaderPtr = NULL;
  iSpeedPtr = NULL;
  iCoordinateElementCount = 0;
  iDriveModeElementCount = 0;
  iHeaderCoordinateElementCount = 0;
  iHeaderDriveModeElementCount = 0;
}
//*****************************************************************************

bool CCoordinateDriveData::SetTimestamp(const Common::TTimestamp &aStamp)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeTimestamp, 
                                  (const char *)&aStamp, sizeof(Common::TTimestamp));
    iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);

    // Mark last element
    SetLastElementType( KTypeTimestamp);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CCoordinateDriveData::SetCoordinate2D(const TCoordinate2D &aCoordinate)
{
  bool result = false;
  if (!IsReadOnlyContainer()) {
    //If there is an header:
    // Check if there is enough coordinate-elements, 
    // if yes print warning and don't add it, if no Add the coordinate-element and increase the lement count
    // 
    if(iCoordinateHeaderPtr != NULL){
      if (iHeaderCoordinateElementCount == iCoordinateElementCount){
        dPrint(1,"Warning!There is already as much TCoordinate2D-structure elements than said in CoordinateHeader(%d), can't add anymore",iHeaderCoordinateElementCount);
      }else{
        const gim::binbag::TBinBlob *b = 
          iBinBagContainer->AddBinary(KTypeCoordinate2D, (const char *)&aCoordinate, 
                                      sizeof(TCoordinate2D));
        
        if (iFirstCoordinatePtr == NULL) {
          iFirstCoordinatePtr = reinterpret_cast<const TCoordinate2D *>(b->data);
          SetLastElementType(KTypeCoordinate2D);
        }
        
        // Increment element count
    
        ++iCoordinateElementCount;
        //if there is enough AFTER adding element, CoordinateDriveData IS valid
        if (iHeaderCoordinateElementCount == iCoordinateElementCount) iValid = true;
        else iValid = false;
        result = true;
      }

      //If there isn't header, can add only one CoordinateElement!
    }else{
      if (iFirstCoordinatePtr == NULL) {
        const gim::binbag::TBinBlob *b = 
          iBinBagContainer->AddBinary(KTypeCoordinate2D, (const char *)&aCoordinate, 
                                      sizeof(TCoordinate2D));
        iFirstCoordinatePtr = reinterpret_cast<const TCoordinate2D *>(b->data);
        iCoordinateElementCount++;
        iValid = true;
        SetLastElementType(KTypeCoordinate2D);
        result = true;
      }else{
        dPrint(1,"There is already one TCoordinate2D-element. If you wan't add multiple Coordinates. Add a TCoordinateHeader first!"); 
      }
    }
   
  }else{
    dPrint(2,"CoordinateDriveData is Read only!");
  }
     
  return result;

}
//*****************************************************************************

bool CCoordinateDriveData::SetTargetLimitDistance(const TTargetLimitDistance &aLimit)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTargetLimitDistancePtr == NULL) {
    if(GetLastElementType() == KTypeUnknown || iValid){
      ADDELEMENT(TTargetLimitDistance, KTypeTargetLimitDistance, iTargetLimitDistancePtr, aLimit);
      SetLastElementType(KTypeTargetLimitDistance);
      result = iValid= true;
    }else{
      dPrint(1,"There is something wrong at CoordinateDriveData, can't add TargetLimitDistance. Last element type %d, Valid %d",
             GetLastElementType(),
             iValid);
    }
  }
  return result;

}
//*****************************************************************************

bool CCoordinateDriveData::SetDriveMode(const TDriveMode &aMode)
{
  bool result = false;
  if (!IsReadOnlyContainer()) {
    //If there is an header:
    // Check if there is enough driveMode-elements, 
    // if yes print warning and don't add it, if no Add the coordinate-element and increase the lement count
    // 
    if(iDriveModeHeaderPtr != NULL){
      if (iHeaderDriveModeElementCount == iDriveModeElementCount){
        dPrint(1,"Warning!There is already as much TDriveMode-structure elements than said in DriveModeHeader(%d), can't add anymore",iHeaderDriveModeElementCount);
      }else{
        const gim::binbag::TBinBlob *b = 
          iBinBagContainer->AddBinary(KTypeDriveMode, (const char *)&aMode, 
                                      sizeof(TDriveMode));
        
        
        if (iDriveModePtr == NULL) {
          iDriveModePtr = reinterpret_cast<const TDriveMode *>(b->data);
        }
        
        // Increment element count
    
        ++iDriveModeElementCount;
        //if there is enough AFTER adding element, CoordinateDriveData IS valid
        if (iHeaderDriveModeElementCount == iDriveModeElementCount) iValid = true;
        else iValid = false;
        result = true;
        SetLastElementType(KTypeDriveMode);
      }

      //If there isn't header, can add only one CoordinateElement!
    }else{
      if (iDriveModePtr == NULL) {
        const gim::binbag::TBinBlob *b = 
          iBinBagContainer->AddBinary(KTypeDriveMode, (const char *)&aMode, 
                                      sizeof(TDriveMode));
        iDriveModePtr = reinterpret_cast<const TDriveMode *>(b->data);
        iDriveModeElementCount++;
        iValid = true;
        result = true;
        SetLastElementType(KTypeDriveMode);

      }else{
        dPrint(1,"There is already one TDriveMode-element. If you wan't add multiple DriveModes. Add a TDriveModeHeader first!"); 
      }
    }
   
  }else{
    dPrint(2,"CoordinateDriveData is Read only!");
  }
     
  return result;
  
}
//*****************************************************************************
bool CCoordinateDriveData::SetCommand(const TCommand &aCommand)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    if(GetLastElementType() == KTypeUnknown || iValid){
      // Use the handy Macro found from MaCIData
      ADDELEMENT(TCommand, KTypeCommand, iCommandPtr, aCommand);
      result = iValid = true;
      SetLastElementType(KTypeCommand);

    }else{
      dPrint(1,"There is something wrong at CoordinateDriveData, can't add Command. Last element type %d, Valid %d",
             GetLastElementType(),
             iValid);
    }
  }
  return result;


}
//*****************************************************************************

bool CCoordinateDriveData::SetCoordinateHeader(const TCoordinateHeader &aHeader)
{
  bool result = false;

  if (!IsReadOnlyContainer() && iCoordinateHeaderPtr == NULL) {
    
    // Use the handy Macro found from MaCIData
    if(GetLastElementType() == KTypeUnknown || iValid){
      ADDELEMENT(TCoordinateHeader, KTypeCoordinateHeader, iCoordinateHeaderPtr, aHeader);
      iHeaderCoordinateElementCount = aHeader.coordinateCount;
      result = true;
      if(aHeader.coordinateCount != 0){
        iValid = false;
      }else{
        iValid = true;
      }
      SetLastElementType(KTypeCoordinateHeader);

    }else{
      dPrint(1,"There is something wrong at CoordinateDriveData, can't add CoordinateHeader. Last element type %d, Valid %d",
             GetLastElementType(),
             iValid);
    }
  }
  return result;
}
//*****************************************************************************

bool CCoordinateDriveData::SetDriveModeHeader(const TDriveModeHeader &aHeader)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iDriveModeHeaderPtr == NULL) {
    // Use the handy Macro found from MaCIData
    if(GetLastElementType() == KTypeUnknown || iValid){
      ADDELEMENT(TDriveModeHeader, KTypeDriveModeHeader, iDriveModeHeaderPtr, aHeader);
      iHeaderDriveModeElementCount = aHeader.driveModeCount;
      result = true;
      if(aHeader.driveModeCount != 0){
        iValid = false;
      }else{
        iValid = true;
      }
      SetLastElementType(KTypeDriveModeHeader);
    
    }else{
      dPrint(1,"There is something wrong at CoordinateDriveData, can't add DriveModeHeader. Last element type %d, Valid %d",
             GetLastElementType(),
             iValid);
    }
          
  }
  return result;
}
//*****************************************************************************

bool CCoordinateDriveData::SetSpeed(const TSpeed &aSpeed)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iSpeedPtr == NULL) {
    if(GetLastElementType() == KTypeSpeed || iValid){
      // Use the handy Macro found from MaCIData
      ADDELEMENT(TSpeed, KTypeSpeed, iSpeedPtr, aSpeed);
      result = iValid = true;
      SetLastElementType(KTypeSpeed);

    }else{
      dPrint(1,"There is something wrong at CoordinateDriveData, can't add TSpeed container. Last element type %d, Valid %d",
             GetLastElementType(),
             iValid);
    }
  }
  return result;
}
//*****************************************************************************

void CCoordinateDriveData::Print(const int level) const
{
 
  const gim::binbag::TBinBlob *b = NULL;
  if(iValid){
    dPrint(level,"CoordinateDriveData is valid ");
  }else{
    dPrint(level,"CoordinateDriveData isn't valid,something is wrong!");
  }
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {

      case KTypeTimestamp: {
	const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
	dPrint(level,"TTimestamp : %d, %d", 
	       ts->timestamp_s,
	       ts->timestamp_us);
	break;
      }

      case KTypeCoordinate2D: {
	const TCoordinate2D *coord = 
          reinterpret_cast<const TCoordinate2D *>(b->data);
	dPrint(level,"TCoordinate2D : x(x,y,a) =(%f, %f, %f), isDifferential %d, isPathTarget %d, isWaitpoint %d", 
	       coord->x,
	       coord->y,
	       coord->a,
	       coord->isDifferential,
	       coord->isPathTarget,
	       coord->isWaitPoint);
	break;
      }

      case KTypeTargetLimitDistance: {
	const TTargetLimitDistance *tld = 
          reinterpret_cast<const TTargetLimitDistance *>(b->data);
	dPrint(level,"TTargetLimitDistance: limit: %f",
	       tld->limit);
        break;
      }

      case KTypeDriveMode:{
	const TDriveMode *mode = 
	  reinterpret_cast<const TDriveMode *>(b->data);
	dPrint(level,"TDriveMode: mode %d",
	       mode->mode);
	break;
      }
      case KTypeCommand: {
        const TCommand *cp = reinterpret_cast<const TCommand *>(b->data);
        dPrint(level,"TCommand: %d",
               cp->cmd);
        break;
      }
      case KTypeCoordinateHeader:{
	const TCoordinateHeader *ch =reinterpret_cast<const TCoordinateHeader *>(b->data);
	dPrint(level,"TCoordinateHeader: number of coordinates %d",
	       ch->coordinateCount);
	break;
      }

      case KTypeDriveModeHeader:{
	const TDriveModeHeader *dh = reinterpret_cast<const TDriveModeHeader *>(b->data);
	dPrint(level,"TDriveModeHeader: number of drive modes %d",
	       dh->driveModeCount);
	break;
      }

      case KTypeSpeed:{
	const TSpeed *s = reinterpret_cast<const TSpeed *>(b->data);
	dPrint(level,"TSpeed: speed %f, angular speed %f",
	       s->speed,
               s->angularSpeed);
	break;

        break;
      }
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
    }
  }
}
//*****************************************************************************

bool CCoordinateDriveData::IsValid()
{
  return iValid;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
#ifdef COMPILE_COORDINATEDRIVEDATA_STANDALONE_MAIN
using namespace MaCI::CoordinateDrive;

int main(void)
{
  debugInit();
  gim::binbag::CBinBag bb;
  CCoordinateDriveData A;
  CCoordinateDriveData B;
  float speed = 0.2, angularSpeed = 1.8;
  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
 
  printf("Encode: Setting Command(SetCoordinate (%d))",KCommandSetCoordinate2D);
  A.SetCommand(TCommand(KCommandSetCoordinate2D));
  printf("OK.\n");

  printf("Encode: Timestamp()\n");
  A.SetTimestamp(MaCI::Common::TTimestamp());
  printf("OK.\n");

  printf("Encode: Setting coordinateHeader(3)");
  A.SetCoordinateHeader(TCoordinateHeader(3));
  printf("OK.\n");

  printf("Encode: Setting one Coordinate(1.1,2.2,3.3,0,0,1)...");
  A.SetCoordinate2D(TCoordinate2D(1.1, 2.2,3.3,0,0,1));
  printf("OK.\n");

  printf("Encode: Setting one Coordinate(1.1,2.2,3.3,0,0,1)...");
  A.SetCoordinate2D(TCoordinate2D(1.1, 2.2,3.3,0,0,1));
  printf("OK.\n");

  printf("Encode: Setting one Coordinate(1.1,2.2,3.3,0,0,1)...");
  A.SetCoordinate2D(TCoordinate2D(1.1, 2.2,3.3,0,0,1));
  printf("OK.\n");

  printf("Encode: Set DriveModeHeader(2)...");
  A.SetDriveModeHeader(2);
  printf("OK.\n");

  printf("Encode: DriveMode (KModeUseReverse(%d))",KModeUseReverse);
  A.SetDriveMode(TDriveMode(KModeUseReverse));
  printf("OK.\n");

  printf("Encode: DriveMode (KModeUseAngle(%d))",KModeUseAngle);
  A.SetDriveMode(TDriveMode(KModeUseAngle));
  printf("OK.\n");

  printf("Encode: TSpeed (%f,%f)",speed,angularSpeed);
  A.SetSpeed(TSpeed(speed,angularSpeed));
  printf("OK.\n")
;
  printf("Let's print CoordinateDriveData (A)..\n");
  A.Print(1);
  
  printf("-----------------------------------------------------\n");
  printf("-----------------------------------------------------\n");
  printf("Decoding...\n");
  // Do Decoding
  B.DecodeFrom(&bb);
  if(B.IsValid()){
    printf("CoordinateData is Valid, let's decode it\n");
    printf("Decode: Commmand (%d)\n",
           B.GetCommand()->cmd);

    printf("Decode: Timestamp = (%d, %d)\n", 
	 B.GetTimestamp()->timestamp_s, 
	 B.GetTimestamp()->timestamp_us);

    printf("Decode: CooordinateHeader, number of coordinates = %d\n",
           B.GetCoordinateHeader()->coordinateCount);
    for(unsigned int i = 0; i< B.GetCoordinateHeader()->coordinateCount; i++){
      printf("Decode: Coordinate (x,y,a) =(%f,%f,%f), Differential %d, pathTarget %d, waitPoint %d\n",
             B.GetCoordinate(i)->x,
             B.GetCoordinate(i)->y,
             B.GetCoordinate(i)->a,
             B.GetCoordinate(i)->isDifferential,
             B.GetCoordinate(i)->isPathTarget,
             B.GetCoordinate(i)->isWaitPoint);
    }

    printf("Decode: DriveModeHeader, number of driveModes %d\n",
           B.GetDriveModeHeader()->driveModeCount);
    for(unsigned int i = 0; i< B.GetDriveModeHeader()->driveModeCount; i++){
      printf("Decode: DriveMode %d\n",
             B.GetDriveMode(i)->mode);
    }

    printf("Decode: Speed, speed %f, angularSpeed %f\n",
           B.GetSpeed()->speed,
           B.GetSpeed()->angularSpeed);
  }else{
    printf("CoordinateDriveData (B) is not Valid!\n");
    B.Print(1);
  }
  
  printf("-----------------------------------------------------\n");
  printf("-----------------------------------------------------\n");
  CCoordinateDriveData C(A); 
  printf("Trying the copy constructor..\n");
  printf("Print data:\n");
  C.Print(1);
  printf("-----------------------------------------------------\n");
  printf("-----------------------------------------------------\n");
  CCoordinateDriveData D = C;
  printf("Trying the = operator\n");
  printf("Print data:\n");
  D.Print(1);
  printf("\nTest complete.\n");
}
#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
